package Model.IntermittentCommunicationModel;

import org.ejml.simple.SimpleMatrix;

import java.util.ArrayList;
import java.util.List;

public class ShipController {

    /*
    * τ(ηi) = −z1i − k1 z2i − ˆρi(t)+ C(˙ηi)˙ηi + D(˙ηi)˙ηi + Mi ˙αi
    *
    * */


    /**
     * 校验矩阵是否有有三行
     * @param v 输入矩阵
     * @return boolean
     */
    public static boolean checkMatrixHasThreeRow(SimpleMatrix v) {
        if (v == null || v.numRows() != 3 || v.numCols() != 1 ) { // 判断参数V是否符合计算条件
            return false;
        }
        return true;
    }


    /**
     * 得到转换坐标所用矩阵， 即R(ψ)
     * 该函数作用是获取 R(ψ)
     * @param ψ ψ
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getTransMatrix(double ψ) {
        // 转换矩阵
        double[][] data = {
                {Math.cos(ψ), -Math.sin(ψ), 0},
                {Math.sin(ψ), Math.cos(ψ), 0},
                {0, 0, 1}
        };
        SimpleMatrix R  = new SimpleMatrix(data);
        return R;
    }

    /**
     * 坐标系变换结果
     * 得到真实位置对时间的导数，即η‘= R(ψ)v
     * R(ψ) 为大地坐标系与船体坐标系的转换矩阵；
     * 该函数作用是获取 R(ψ)v
     * @param ψ ψ为大地坐标系下的艏摇角
     * @param v v=[u,v,r]T 为船舶在船体坐标系下的纵荡、横荡和艏摇角速度
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getTansPosition(double ψ, SimpleMatrix v) {
        if (!checkMatrixHasThreeRow(v)) {
            return null;
        }

        // 调用转换矩阵计算结果
        SimpleMatrix R  = getTransMatrix(ψ);
        return R.mult(v);
    }

    public static SimpleMatrix MTrans; // 质量矩阵M的转置矩阵

    /**
     * 得到加速度
     * 即˙˙η
     * @return {@link SimpleMatrix}
     */
//    public static SimpleMatrix getAccelerateSpeed(
//            BaseShip ship
//    ) {
//        return ship.getAccelerateSpeed();
////        SimpleMatrix vMatrix = ship.vMatrix;
////        double ψ = ship.ψ;
////        SimpleMatrix firstPart = getTransMatrix(ψ).mult(MTrans).mult(ship.τMatrix.plus(ship.τbMatrix));
////
////        SimpleMatrix secondPart;
////        double r = vMatrix.get(2,0);
////        double[][] S = {
////                {0, -r, 0},
////                {r, 0, 0},
////                {0, 0, 0}
////        };
////        secondPart = new SimpleMatrix(S).mult(getTansPosition(ψ, vMatrix));
////
////        double[][] RTData = {
////                {Math.cos(ψ), Math.sin(ψ), 0},
////                {-Math.sin(ψ), Math.cos(ψ), 0},
////                {0, 0, 1}
////        };
////        SimpleMatrix RTMatrix  = new SimpleMatrix(RTData);
////        // R*M*(Cv + Dv)*R^T*(˙η)
////        SimpleMatrix RMatrix =  getTransMatrix(ψ).mult(MTrans).mult(getCoriolisMatrix(vMatrix).plus(getDampMatrix(vMatrix))).mult(RTMatrix).mult(getTansPosition(ψ, vMatrix));
////        secondPart.minus(RMatrix);
////
////        return firstPart.plus(secondPart);
//    }


    /**
     * 得到加速度
     * 使用论文中逆推得到的公式
     * 即˙˙η
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getNewAccelerateSpeed(
            SimpleMatrix virtualControl
    ) {
         // M^-1
        SimpleMatrix M = getMassMatrix().invert();
        return M.mult(virtualControl);
    }


    /**
     * 得到科里奥利向心矩阵C(v)
     * TODO：该矩阵适用船舶模型为： Cybership II
     * @param v 摇角速度v=[u,v,r]T ，分别为为船舶在船体坐标系下的纵荡、横荡和艏
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getCoriolisMatrix(SimpleMatrix v) {
        if (!checkMatrixHasThreeRow(v)) {
            return null;
        }
        double uD = v.get(0, 0);
        double vD = v.get(1, 0);
        double rD = v.get(2, 0);
        //初始化一个矩阵，并进行下面的赋值
        double[][] data = {
                {0, 0, -(33.8 * vD) - (1.0115 * rD)},
                {0, 0, 25.8 * uD},
                {(33.8 * vD) + (1.0115 * rD), -25.8 * uD, 0}
        };
        SimpleMatrix matrix  = new SimpleMatrix(data);
//        System.out.println("getCoriolisMatrix" + matrix);
        return matrix;
    }

    /**
     * 得到阻尼矩阵D（v）
     * TODO: 该质量矩阵适用船舶模型为： Cybership II
     * @param v v=[u,v,r]T ，分别为为船舶在船体坐标系下的纵荡、横荡和艏摇角速度
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getDampMatrix(SimpleMatrix v) {
        if (!checkMatrixHasThreeRow(v)) {
            return null;
        }
        //初始化一个矩阵，并进行下面的赋值
        double uD = v.get(0, 0);
        double vD = v.get(1, 0);
        double rD = v.get(2, 0);
        double[][] data = {
                {0.72 + 1.33 * Math.abs(uD), 0, 0},
                {0, 0.89 + 36.5 * Math.abs(vD) + 0.8 * Math.abs(rD), 7.3 + 0.85 * Math.abs(vD) + 3.5 * Math.abs(rD)},
                {0, 0.31 + 3.96 * Math.abs(vD) - 0.13* Math.abs(rD), 1.9 - 0.08 * Math.abs(vD) + 0.75 * Math.abs(rD)}
        };
        SimpleMatrix matrix  = new SimpleMatrix(data);
        return matrix;
    }

    /**
     * 得到质量矩阵 Mi
     * TODO: 该质量矩阵适用船舶模型为： Cybership II
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getMassMatrix() {
        double[][] data = {
                {25.8, 0, 0},
                {0, 33.8, 1.0115},
                {0, 1.0115, 2.76}
        };
        SimpleMatrix matrix  = new SimpleMatrix(data);
        return matrix;
    }

    /**
     * 得到：σi0 = Ri(ψi −ψ0i)η0
     * 即：获得 firstShipAngle的船舶在编队中的位置参考向量
     * @param firstShipAngle     第一船 艏摇角 ψi
     * @param secondShipAngle    第二船 艏摇角 ψ0i 或 ψji
     * @param idealPosition           理想轨迹信号 η0
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getPositionWithTwoShips(double firstShipAngle,
                                                       double secondShipAngle,
                                                       SimpleMatrix idealPosition
    ) {
        if (!checkMatrixHasThreeRow(idealPosition))
            return null;
        SimpleMatrix R = getTransMatrix(firstShipAngle - secondShipAngle);
        return R.mult(idealPosition);
    }


    /**
     * TODO σij的计算不确定
     * 得到位置跟踪误差
     * 得到： z1i = N∑j=0 aij (ηi −ηj −σij)
     * 即 公式(3)
     * 含义为：得到某船的位置跟踪误差
     * @param shipPositionInList 船在列表中的位置
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getPositionTrackingError(int shipPositionInList) {
        if (shipsCommunicationState.numRows() <= shipPositionInList) return null;

        SimpleMatrix result = new SimpleMatrix(3, 1);
        BaseShip nowShip = baseShips.get(shipPositionInList);
        for(int j = 0; j < baseShips.size(); j++) {
            SimpleMatrix flag;
            BaseShip targetShip = baseShips.get(j);
            // 该船与虚拟领航船的相对位置误差：aij*(ηi −ηj −σij)
            // aij从连通矩阵中读取，
            // ηi及ηj分别从对应位置的船，自身存储的数据中读取
            // σij根据 是否与领航船之间的距离 判断，若与领航船，则直接读取自身存储的数据；否则由公式计算得出
            if (targetShip.shipType.equals(ShipType.follower)) {
//                flag = nowShip.getCurrentPosition().minus(targetShip.getCurrentPosition()).minus(getPositionWithTwoShips(
//                        nowShip.idealPositionMatrix.get(2,0),
//                        targetShip.idealPositionMatrix.get(2,0),
//                        nowShip.idealPositionMatrix
//                        ));
                flag = nowShip.getCurrentPosition().minus(targetShip.getCurrentPosition()).minus(
                        nowShip.idealPositionMatrix.minus(targetShip.idealPositionMatrix)
//                        targetShip.idealPositionMatrix.minus(nowShip.idealPositionMatrix)
                );
            } else {
                flag = nowShip.getCurrentPosition().minus(targetShip.getCurrentPosition()).minus(nowShip.idealPositionMatrix);
            }
            flag = flag.scale(shipsCommunicationState.get(shipPositionInList, j));
//            System.out.println("船" + nowShip.shipNum + " 与船" + targetShip.shipNum + "的flag: " + flag);
            result = result.plus(flag);
        }
        return result;
    }

    /**
     * TODO: 对角正定矩阵 λ 是否正确
     * 得到虚拟控制矩阵
     * 得到： αi = N∑j=0 ( aij *˙ηj) − λi * z1i
     * @param shipPositionInList 船在列表中的位置
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getVirtualControlMatrix(int shipPositionInList) {
        if (shipsCommunicationState.numRows() <= shipPositionInList) return null;

        SimpleMatrix result = new SimpleMatrix(3, 1);

        // 求得：∑j=0 aij ˙ηj
        for (int j = 0; j < baseShips.size(); j++) {
            SimpleMatrix flag;
            flag = baseShips.get(j).getTansPositionSpeed().scale(shipsCommunicationState.get(shipPositionInList, j));
            result = result.plus(flag);
        }

        // 求得： αi = ( N∑j=0 aij ˙ηj)−λiz1i
        result = result.minus(λi.mult(getPositionTrackingError(shipPositionInList)));

//        System.out.println("船" + shipPositionInList + " 在时间 :" + nowTime + "的αi为：" + result);
        return result;
    }

    /**
     * 得到速度跟踪误差
     * 得到： z2i = −αi + N∑j=0（aij * ˙ηi）
     * @param shipPositionInList 船在列表中的位置
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getSpeedTrackingError(int shipPositionInList) {
        if (shipsCommunicationState.numRows() <= shipPositionInList) return null;

        // 首先获取虚拟控制率
        SimpleMatrix result = getVirtualControlMatrix(shipPositionInList);
//        System.out.println("虚拟控制率：" + result);
        result = result.scale(-1); // −αi

        // 计算累加和： ∑j=0 aij ˙ηi => R(ψ)*v*aij
        for (int j = 0; j < baseShips.size(); j++) {
            SimpleMatrix flag;
            flag = baseShips.get(shipPositionInList).getTansPositionSpeed().scale(shipsCommunicationState.get(shipPositionInList, j));
            result = result.plus(flag);
        }

//        System.out.println("船" + shipPositionInList + " 在时间 :" + nowTime + "的 z2i为：" + result);

        return result;
    }


    public static double nowTime = 0; // 当前时间，用于根据领航船位移函数计算当前位置坐标
    // 由于控制率计算需要对轨迹函数进行求导
    // 领航船的轨迹函数进行求导，及二阶导数，分为两种情况：
    // 1、必须得知轨迹函数，在这里直接写出对应的1，2阶导数，并根据时间可得到对应数据
    // 2、短程距离可近似为直线，故一阶导数可视为常数，二阶导数视为0
    // TODO：

    /**
     * // TODO  在这里固定为论文中预设的误差观测器 ρi(t)=[500 sin(t),1 200,100 cos(t)]T
     * 得到环境误差矩阵
     * @param t t 误差观测器
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getDisturbanceObserver(double t) {
        double[][] data = {{500 * Math.sin(t)}, {1200}, {100 * Math.cos(t)}};
        return new SimpleMatrix(data);
    }

    /**
     * 微分位置跟踪误差
     * 位置跟踪误差的时间导数结果
     * 即得到： ˙z1i = z2i −λiz1i
     * @param shipPositionInList 船在列表的位置
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getDifferentialOfPositionTrackingError(int shipPositionInList) {
        if (shipsCommunicationState.numRows() <= shipPositionInList) return null;

        SimpleMatrix result = λi.mult(getPositionTrackingError(shipPositionInList));
//
//        System.out.println("船" + shipPositionInList + " 在时间 :" + nowTime + "的˙z1i为：" + getSpeedTrackingError(shipPositionInList).minus(result));

        return getSpeedTrackingError(shipPositionInList).minus(result);
    }

    /**
     * 微分扰动观测器
     * 得到扰动观测器的对时间的积分
     * 即： ˙αi
     * @param shipPositionInList 船在列表的位置
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getDifferentialOfDisturbanceObserver(int shipPositionInList) {

        SimpleMatrix result = new SimpleMatrix(3,1);

        BaseShip nowShip = baseShips.get(shipPositionInList);
        for (int j = 0; j < baseShips.size(); j++) {
            BaseShip targetShip = baseShips.get(j);
            SimpleMatrix flag = targetShip.getAccelerateSpeed().minus(nowShip.getTansPositionSpeed()).plus(targetShip.getTansPositionSpeed());
            result = result.plus(flag.scale(shipsCommunicationState.get(shipPositionInList, j)));
        }

        // 对该项 −λi * z1i 求导的结果：
//        SimpleMatrix result = λi.mult(getDifferentialOfPositionTrackingError(shipPositionInList)).scale(-1);
//
//        // 计算 ∑j=0 aij * ˙ηj 的求导累加和
//        for (int j = 0; j < baseShips.size(); j++) {
//            SimpleMatrix flag;
//            flag = baseShips.get(j).getAccelerateSpeed().scale(shipsCommunicationState.get(shipPositionInList, j));
//            result = result.plus(flag);
//        }
//
////        System.out.println("船" + shipPositionInList + " 在时间 :" + nowTime + "的˙αi为：" + result);
//
        return result;
    }

    static SimpleMatrix θMatrix; //  θ为中间辅助向量；
    static SimpleMatrix ke; // kei = diag(325,325,325)

    /**
     * 得到误差观测器
     * ρi(t) = θ + ke * Mi * ˙ηi
     */
    public static SimpleMatrix getErrorObserver(int shipPositionInList) {
        return θMatrix.plus(
                ke.mult(getMassMatrix()).mult(baseShips.get(shipPositionInList).getTansPositionSpeed())
        );
    }


    // τ(ηi) = −z1i − k1 z2i − ˆρi(t)+ C(˙ηi)˙ηi + D(˙ηi)˙ηi + Mi ˙αi

    /**
     * 获取船的控制矩阵
     * @param shipPositionInList 船位置在列表
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix getControlMatrix(int shipPositionInList) {
        BaseShip nowShip = baseShips.get(shipPositionInList);

        SimpleMatrix z1 = getPositionTrackingError(shipPositionInList).scale(-1); //  −z1i
//        System.out.println("z1:" + z1);

        SimpleMatrix k1z2i = k1.mult(getSpeedTrackingError(shipPositionInList)).scale(-1); // −k1 * z2i
//        System.out.println("k1z2i:" + k1z2i);

        SimpleMatrix disturb = getDisturbanceObserver(nowTime).scale(-1); // −ˆρi(t)
//        SimpleMatrix disturb = getErrorObserver(shipPositionInList).scale(-1); // −ˆρi(t)
//        System.out.println("disturb----- −ˆρi(t):" + disturb);

        SimpleMatrix coriolis = getCoriolisMatrix(nowShip.getTansPositionSpeed()).mult(nowShip.getTansPositionSpeed());// + C(˙ηi)˙ηi
//        System.out.println("coriolis----- C(˙ηi)˙ηi:" + coriolis);

        SimpleMatrix damp = getDampMatrix(nowShip.getTansPositionSpeed()).mult(nowShip.getTansPositionSpeed()); // + D(˙ηi)˙ηi
//        System.out.println("damp-----  D(˙ηi)˙ηi:" + damp);

        SimpleMatrix mass = getMassMatrix().mult(getDifferentialOfDisturbanceObserver(shipPositionInList)); // + Mi ˙αi
//        System.out.println("mass-----  Mi ˙αi:" + mass);

        return z1.plus(k1z2i).plus(disturb).plus(coriolis).plus(damp).plus(mass);
    }

    /**
     * 计算加速度
     *
     * @param shipPositionInList 船位置在列表
     * @return {@link SimpleMatrix}
     */
    public static SimpleMatrix calculateAccelerateSpeed(int shipPositionInList) {
        BaseShip nowShip = baseShips.get(shipPositionInList);

//        System.out.println("z1:" + getPositionTrackingError(shipPositionInList));
        SimpleMatrix z1 = getPositionTrackingError(shipPositionInList).scale(-1); //  −z1i
//        System.out.println("-z1:" + z1);

        SimpleMatrix k1z2i = k1.mult(getSpeedTrackingError(shipPositionInList)).scale(-1); // −k1 * z2i
//        System.out.println("k1z2i:" + k1z2i);

//        SimpleMatrix disturb = getDisturbanceObserver(nowTime).scale(-1); // −ˆρi(t)
        SimpleMatrix disturb = getErrorObserver(shipPositionInList).scale(-1); // −ˆρi(t)
//        System.out.println("disturb----- −ˆρi(t):" + disturb);

//        SimpleMatrix coriolis = getCoriolisMatrix(nowShip.getTansPositionSpeed()).mult(nowShip.getTansPositionSpeed());// + C(˙ηi)˙ηi
//        System.out.println("coriolis----- C(˙ηi)˙ηi:" + coriolis);

//        SimpleMatrix damp = getDampMatrix(nowShip.getTansPositionSpeed()).mult(nowShip.getTansPositionSpeed()); // + D(˙ηi)˙ηi
//        System.out.println("damp-----  D(˙ηi)˙ηi:" + damp);

        SimpleMatrix mass = getMassMatrix().mult(getDifferentialOfDisturbanceObserver(shipPositionInList)); // + Mi ˙αi
//        System.out.println("mass-----  Mi ˙αi:" + mass);
//
        SimpleMatrix environmentDis = getDisturbanceObserver(nowTime); // 环境误差矩阵

        SimpleMatrix result = z1.plus(k1z2i).plus(disturb).plus(environmentDis).plus(mass); // 右侧方程
//        System.out.println("右侧方程:" + result);

        System.out.println("计算出的加速度：：" + getNewAccelerateSpeed(result) );

        return getNewAccelerateSpeed(result);
    }


    // 存储舰队中所有船的信息的列表
    // 默认第一个船是领航船
    public static List<BaseShip> baseShips = new ArrayList<>(); // 初始化后不可修改

    // 存储舰队中所有船的 互相通信情况的列表，每一项代表通信无向图中的一条边
    // 后续会根据该通信情况自动生成邻接矩阵
//    public static List<> communicationStateSide = new ArrayList<>();

    // 存储舰队中所有船的通信情况的邻接矩阵，
    // 邻接矩阵每行依次和船队列表中所代表的船一致
    // 默认领航船位于 0 处
    public static SimpleMatrix shipsCommunicationState = new SimpleMatrix(); // 初始化后不可修改

    static SimpleMatrix λi; // 计算所用常量
    static SimpleMatrix k1;
    /**
     * init船舶数据： 初始化船队通讯情况 ，及各个船的初始信息
     * TODO: λi 可能需要修改
     */
    // 初始位置为：
    // η1(0) = [0,50, π/2]T,
    // η2(0) = [0,0, π/3]T,
    // η3(0) = [50,0, π/4]T,
    // η4(0) = [50,50, π/5]T
    // 与领航者的理想相对位置：
    // σ10 = [0,0,0]T,
    // σ20 = [0,−50,0]T,
    // σ30 = [0,50,0]T,
    // σ40 = [0,−100,0]T
    public static void initShipsData() {
        baseShips = new ArrayList<>();
        // 设定初始位置 预期位置
        BaseShip virtualLeaderShip = new BaseShip(0, 0, 0);
//        virtualLeaderShip.setActualPosition(0, 0, 0);
        BaseShip baseShipFirst = new BaseShip(0,50, Math.PI / 2, 0,0,0);
        BaseShip baseShipSecond = new BaseShip(0,0, Math.PI / 3, 0,-50,0);
        BaseShip baseShipThird = new BaseShip(50,0, Math.PI / 4, 0,50,0);
        BaseShip baseShipForth = new BaseShip(50,50, Math.PI / 5, 0,-100,0);
        baseShips.add(virtualLeaderShip);
        baseShips.add(baseShipFirst);
        baseShips.add(baseShipSecond);
        baseShips.add(baseShipThird);
        baseShips.add(baseShipForth);

        // 设定M的逆矩阵
        double[][] MTransData = {
                {0.038759689922480620155, 0, 0},
                {0, 0.029913878026449563734, -0.010963002762229613665},
                {0, -0.010963002762229613665, 0.36633662220796929501}
        };
        MTrans = new SimpleMatrix(MTransData);

        // 设定通讯关系
        double[][] communication = {
                {0, 1, 1, 0, 0},
                {1, 0, 0, 1, 0},
                {1, 0, 0, 0, 1},
                {0, 1, 0, 0, 0},
                {0, 0, 1, 0, 0}
        };
//        double[][] communication = {
//                {2, -1, -1, 0, 0},
//                {-1, 2, 0, -1, 0},
//                {-1, 0, 2, 0, -1},
//                {0, -1, 0, 1, 0},
//                {0, 0, -1, 0, 1}
//        };
        shipsCommunicationState = new SimpleMatrix(communication);


        double[][] data = {
                {125, 0, 0},
                {0, 125, 0},
                {0, 0, 125}
        };
        λi = new SimpleMatrix(data); // λi ∈ R3×3 为对角正定矩阵。


        double[][] k1Data = {
                {50, 0, 0},
                {0, 50, 0},
                {0, 0, 50}
        };
        k1 = new SimpleMatrix(k1Data); // k1 = diag(50,50,50)

//        double[][] θData = {
//                {0, 0, 0},
//                {0, 0, 0},
//                {0, 0, 0}
//        };
        double[][] θData = {
                {0},
                {0},
                {0}
        };
        θMatrix = new SimpleMatrix(θData); // θ∈ R3×3,为中间辅助向量；

        double[][] KeData = {
                {325, 0, 0},
                {0, 325, 0},
                {0, 0, 325}
        };
        ke = new SimpleMatrix(KeData); // kei = diag(325,325,325)
    }

    /**
     * 设置时间
     *
     * @param time 时间
     */
    public static void setTime(double time) {
        nowTime = time;
        BaseShip.nowTime = time;
    }


}
















